﻿using System;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Input;
using System.Diagnostics;

using FarseerGames.FarseerPhysics;
using FarseerGames.FarseerPhysics.Collisions;
using FarseerGames.FarseerPhysics.Dynamics;
using FarseerGames.FarseerPhysics.Factories;

namespace Organized
{

    public class PhysicsTerrainActor : PhysicsActor
    {
        // how is the vehicle oriented is calculated based on the user's input and
        // a heightmap's normals, and then used when drawing.
        public Matrix orientation = Matrix.Identity;
        Vector3 oldUpVector = Vector3.Zero;
        

        //override Actors Rotation Matrix with our own orientation matrix
        public override Matrix Rotation
        {
            get
            {
                return orientation;
            }
            set
            {
                orientation = value;
            }
        }

        public float YPosition;

        public Vector3 oldPosition;

        public override Vector3 Position
        {
            set { position = new Vector3(value.X, YPosition, value.Z); }
            get { return position; }
        }

        private Vector3 position;
        public bool Airborne = false;

        public float Vspeed;

        HeightMapData heightMapData;

        private bool PhysicsEnabled = true;

        public bool floorAttached = true;

        float maxFallSpeed = -40;
        float gravity = 0.6f;

        //used to calculate future position
        private Vector3 newPosition = Vector3.Zero;
        //particle's velocity
        public Vector2 ParticleVelocity;
        Vector3 normal;

        //our directional speed
        public float ZSpeed;

        //universal 'average' radius for small objects and big objects (to handle Yposition layer collision)
        public float GlobalSmallObjectRadius;
        public float GlobalBigObjectRadius;
        //used to calculate Y position offset from terrain height
        float heightFromTerrain;
        public bool freeMovement = false;

        public PhysicsTerrainActor(Model Model, Lighting _light, PhysicsParticle _physicsParticle, Vector3 _position, GameScreen Parent) : base (Model,_light,_physicsParticle, _position,Parent)
        {
            Position = _position;
            YPosition = _position.Y;
            
            oldPosition = Position;
          
        }

    


        public void SetHeightMapData(HeightMapData _heightMapData)
        {

            heightMapData = _heightMapData;
        }

        
        public override void Update()
        {

            
               

                //only do calculations if we are moving                

                //calculate speed from orientation
                orientation = Matrix.CreateRotationY(FacingDirection);

                ///PREDICT FUTURE POSITION
                //get particles velocity
                ParticleVelocity = PhysicsParticle.Body.GetVelocityAtLocalPoint(new Vector2(0, 0));
                //calculate new position from particle velocity an current position
                newPosition = Position + new Vector3(ParticleVelocity.X, Vspeed, ParticleVelocity.Y);               

                //get our speed as
                ZSpeed = Vector2.Distance(new Vector2(newPosition.X, newPosition.Y), new Vector2(position.X, position.Y));
                Vector3 testPosition = newPosition;
                
                //check if we are on terrain, to Set orientation
                if (heightMapData.IsOnHeightmap(testPosition))
                {

                    //height and normal at this position.                    
                    heightMapData.GetHeightAndNormal(testPosition, out testPosition.Y, out normal);


                    orientation.Up = oldUpVector;
                }
             



                oldPosition = Position; 
                //if we are attached to the floor (im a cheep), copy y position, or we are going uphill
                if (floorAttached)
                {
                    oldUpVector += ((normal * -1) - oldUpVector) / 4;
                    
                    YPosition = testPosition.Y;
                    //PhysicsParticle.Body.ApplyForce(new Vector2(0, 1));
                }
                else
                {
                    
                    //im a vehicle or something and want to be able to jump 
                    //keep track of Y difference and vertical speed
                    //MOVING UPHILL
                    if (testPosition.Y > (position.Y + Vspeed))
                    {
                        
                        if (Airborne)
                        {
                            Airborne = false;
                            //reset ground friction
                            PhysicsParticle.Body.LinearDragCoefficient = PhysicsParticle.GroundFriction;
                 
                            //also put me in a different collision category, so we dont collide with animals
                            PhysicsParticle.SetCollideWithFromHeight(0);
                        }

                        if (freeMovement)
                        {
                            YPosition = testPosition.Y;
                        }
                                         

                        Vspeed = testPosition.Y - YPosition;
                       
                        oldUpVector += ((normal * -1) - oldUpVector) / 4;
                        
                        orientation.Up = oldUpVector;

                    }
                    else
                    {
                        if (!Airborne && !freeMovement)
                        {
                          
                            Airborne = true;
                            //we are airborne so stop fake ground friction
                            PhysicsParticle.Body.LinearDragCoefficient = 0.1f;
                        
                        }
                            //check my hight from ground, if higher than small objects, do not collide
                            //if higher than big objects, do not collide at all
                            
                            heightFromTerrain = (position.Y + Vspeed) - testPosition.Y;

                            //set our 'collide with' categories
                            PhysicsParticle.SetCollideWithFromHeight(heightFromTerrain);
                        

                        // smoothly interpolate to stable orientation
                        oldUpVector += (new Vector3(0, 1, 0) - oldUpVector) / 50;
                        orientation.Up = oldUpVector;
                        
                       


                    }


                    

                       

                      
                    
                        YPosition += Vspeed;
                        Vspeed -= gravity;
                        if (Vspeed < maxFallSpeed) { Vspeed = maxFallSpeed; };
                   
                }  

               
                    
                    orientation.Right = Vector3.Cross(orientation.Forward, orientation.Up);
                    orientation.Right = Vector3.Normalize(orientation.Right);

                    orientation.Forward = Vector3.Cross(orientation.Up, orientation.Right);
                    orientation.Forward = Vector3.Normalize(orientation.Forward);

          // }

            //update model position from particle
            base.Update();

           
        }

        private void SetPhysics(string state)
        {
            switch (state)
            {

                case "ON":
                    PhysicsEnabled = true;
                    PhysicsParticle.Body.IsStatic = false;
                    PhysicsParticle.Body.Enabled = true;
                    PhysicsParticle.Geom.CollisionEnabled = true;
                    this.Enabled = true;
                  

                    break;
                case "OFF":
                    PhysicsEnabled = false;
                    PhysicsParticle.Body.IsStatic = true;
                    PhysicsParticle.Body.Enabled = false;
                    PhysicsParticle.Geom.CollisionEnabled = false;
                    PhysicsParticle.Geom.Body.ClearForce();
                    this.Enabled = false;
                    break;
            }
        }

    }


}